#include <pmon.h>
static void *gmacbase = (void *)GMAC_CONNECT_TO_SWITCH;

static unsigned int read_phy(int phy, int reg)
{
*(volatile int *)(gmacbase+0x10) = (phy<<11)|(reg<<6)|1|0x10;
while(*(volatile int *)(gmacbase+0x10)&1);
return *(volatile int *)(gmacbase+0x14);
}

static int write_phy(int phy, int reg, unsigned int data)
{
*(volatile int *)(gmacbase+0x14) = data;
*(volatile int *)(gmacbase+0x10) = (phy<<11)|(reg<<6)|2|1|0x10;
while(*(volatile int *)(gmacbase+0x10)&1);
return 0;
}

int  write_switch(int addr, unsigned long long data)
{
int page, reg;

page = addr>>8;
reg = addr&0xff;

write_phy(30, 24, data&0xffff);
write_phy(30, 25, (data>>16)&0xffff);
write_phy(30, 26, (data>>32)&0xffff);
write_phy(30, 27, (data>>48)&0xffff);
write_phy(30, 16, 1|(page<<8));
write_phy(30, 17, 1|(reg<<8));
while(read_phy(30, 17)&3);
}

unsigned long long read_switch(int addr)
{
int page, reg;
unsigned long long val = 0;
page = addr>>8;
reg = addr&0xff;

write_phy(30, 16, 1|(page<<8));
write_phy(30, 17, 2|(reg<<8));
while(read_phy(30, 17)&3);

val |= (unsigned long long)read_phy(30, 24);
val |= (unsigned long long)read_phy(30, 25)<<16;
val |= (unsigned long long)read_phy(30, 26)<<32;
val |= (unsigned long long)read_phy(30, 27)<<48;

return val;
}

int switch_init()
{
write_switch(0x8, 0x1c);
write_switch(0xe, 0x8b);
//write_switch(0xe, 0x87);
write_switch(0x2888, 0);
write_switch(0x200, 0x82);
write_switch(0xb, 7);
write_switch(0x60, 3);
write_switch(0x0203, 0x2);
}

